function q = RotationMatrixToQuat(R)
tr = trace(R);
q = zeros(4,1);
if (tr > 0)
  S = sqrt(tr+1.0) * 2; % S=4*q(1) 
  q(1) = 0.25 * S;
  q(2) = (R(3,2) - R(2,3)) / S;
  q(3) = (R(1,3) - R(3,1)) / S; 
  q(4) = (R(2,1) - R(1,2)) / S; 
elseif ((R(1,1) > R(2,2))&(R(1,1) > R(3,3))) 
  S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; %S=4*q(2) 
  q(1) = (R(3,2) - R(2,3)) / S;
  q(2) = 0.25 * S;
  q(3) = (R(1,2) + R(2,1)) / S; 
  q(4) = (R(1,3) + R(3,1)) / S; 
elseif (R(2,2) > R(3,3))  
  S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*q(3)
  q(1) = (R(1,3) - R(3,1)) / S;
  q(2) = (R(1,2) + R(2,1)) / S; 
  q(3) = 0.25 * S;
  q(4) = (R(2,3) + R(3,2)) / S; 
else 
  S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*q(4)
  q(1) = (R(2,1) - R(1,2)) / S;
  q(2) = (R(1,3) + R(3,1)) / S;
  q(3) = (R(2,3) + R(3,2)) / S;
  q(4) = 0.25 * S;
end
if(q(1) < 0)
    q = -q;
end
if(abs(norm(q) - 1) > 1e-1)
    x = 0;
end
end